#include "fport.h"

#include <stdlib.h>

using namespace NXMFC;

static void copy_bits11(uint16_t &dst, uint8_t *src, unsigned pos)
{
    uint32_t tmp;
    unsigned b;

    b = pos/8;
    tmp = (src[b+2] << 16) | (src[b+1] << 8) | src[b];
    b = pos%8;

    dst = (tmp >> b) & 0x7ff;
}

fport::fport()
{
    buf = 0;
    pos = 0;
    done = 0;
    escaped = 0;
    bufsz = 0;
}

fport::~fport()
{
    if (buf)
        free(buf);
}

bool fport::allocBuf(int size)
{
    if (buf)
        free(buf);
    buf = (uint8_t *)malloc(size);
    if (buf)
        bufsz = uint8_t(size);
    else
        bufsz = 0;

    return (buf != 0);
}

fport::RecvResult fport::recv(rttSerial &iodev, int &type, int timeout_ms)
{
    uint8_t ch;
    int ret;

    if (!buf)
    {
        return FPRR_EIO;
    }

    ret = iodev.readAll(&ch, 1, timeout_ms);
    if (ret < 0)
    {
        return FPRR_EIO;
    }
    if (ret == 0)
    {
        return FPRR_CONTINUE;
    }

    fport::RecvResult rr = putChar(ch);
    if (rr == FPRR_DONE)
        type = buf[2];

    return rr;
}

fport::RecvResult fport::putChar(uint8_t ch)
{
    fport::RecvResult ret = FPRR_CONTINUE;

    if (done)
    {
        pos = 0;
        done = 0;
        buf[1] = 0;
    }

    if (pos < 2 && ch == 0x7E)
        pos = 0;

    if (ch == 0x7d)
    {
        escaped = true;
        goto out;
    }

    if (escaped)
    {
        ch += 0x20;
    }

    if (pos >= bufsz)
    {
        pos = 0;
        ret = FPRR_EFULL;
        goto out;
    }

    buf[pos] = ch;
    pos++;

    if (escaped)
    {
        escaped = false;
        goto out;
    }

    if (pos > 2 && ch == 0x7E)
    {
        if (buf[1] != pos - 4)
        {
            buf[0] = ch;
            pos = 1;
            ret = FPRR_EFM;
            goto out;
        }

        if (checkSum(&buf[1], pos - 3) != buf[pos - 2])
        {
            buf[0] = ch;
            pos = 1;
            ret = FPRR_EFM;
        }
        else
        {
            done = 1;
            ret = FPRR_DONE;
        }
    }

out:
    return ret;
}

bool fport::getChannel(unsigned chn, uint16_t &d, uint16_t *raw)
{
    uint8_t *data = &buf[3];

    if (chn > 15)
        return false;
    if (!done || buf[2] != 0)
        return false;

    copy_bits11(d, data, chn * 11);
    if (raw)
        *raw = d;

    d = d/1.6f + 880;

    return true;
}

uint8_t fport::checkSum(uint8_t *d, int len)
{
    uint16_t s = 0;

    for (int i = 0; i < len; i++)
    {
        s += d[i];
    }

    while (s > 0xFF)
    {
        s = (s & 0xFF) + (s >> 8);
    }
    s = 0xff - s;

    return uint8_t(s);
}
